#include "foc.h"
#include "tim.h"
#include "math.h"
float voltage_power_supply=12.6;
float shaft_angle=0,open_loop_timestamp=0;
float zero_electric_angle=0,Ualpha,Ubeta=0,Ua=0,Ub=0,Uc=0,dc_a=0,dc_b=0,dc_c=0;
int pole_pairs=7;
int PP=1;
void FOC_update_raw_angel(float raw_angel)
{
  shaft_angle = raw_angel;
}
float FOC_get_raw_angel()
{
  return shaft_angle;
}
float _electricalAngle_test(){
  return  _normalizeAngle(PP * shaft_angle*pole_pairs-zero_electric_angle);
}
//进行力矩控制
void FOC_setTorque(float Uq)
{
  //float angle_el = _normalizeAngle(_electricalAngle(shaft_angle,pole_pairs)*1 - zero_electric_angle);
  float angle_el = _electricalAngle_test();
  // 帕克逆变换
   Ualpha =  -Uq*sin(angle_el); 
  Ubeta =   Uq*cos(angle_el); 

  // 克拉克逆变换
  Ua = Ualpha + voltage_power_supply/2;
  Ub = (_SQRT3*Ubeta-Ualpha)/2 + voltage_power_supply/2;
  Uc = (-Ualpha-_SQRT3*Ubeta)/2 + voltage_power_supply/2;
  setPwm(Ua,Ub,Uc);
}


float _normalizeAngle(float angle){
  float a = fmod(angle, 2*_PI);   //取余运算可以用于归一化，列出特殊值例子算便知
  return a >= 0 ? a : (a + 2*_PI);  
  //三目运算符。格式：condition ? expr1 : expr2 
  //其中，condition 是要求值的条件表达式，如果条件成立，则返回 expr1 的值，否则返回 expr2 的值。可以将三目运算符视为 if-else 语句的简化形式。
  //fmod 函数的余数的符号与除数相同。因此，当 angle 的值为负数时，余数的符号将与 _2PI 的符号相反。也就是说，如果 angle 的值小于 0 且 _2PI 的值为正数，则 fmod(angle, _2PI) 的余数将为负数。
  //例如，当 angle 的值为 -PI/2，_2PI 的值为 2PI 时，fmod(angle, _2PI) 将返回一个负数。在这种情况下，可以通过将负数的余数加上 _2PI 来将角度归一化到 [0, 2PI] 的范围内，以确保角度的值始终为正数。
}

//uq,ud,电角度
void setPhaseVoltage(float Uq,float Ud, float angle_el)
{
  angle_el = _normalizeAngle(angle_el + zero_electric_angle);
  // 帕克逆变换
  Ualpha =  -Uq*sin(angle_el); 
  Ubeta =   Uq*cos(angle_el); 

  // 克拉克逆变换
  Ua = Ualpha + voltage_power_supply/2;
  Ub = (_SQRT3*Ubeta-Ualpha)/2 + voltage_power_supply/2;
  Uc = (-Ualpha-_SQRT3*Ubeta)/2 + voltage_power_supply/2;
  setPwm(Ua,Ub,Uc);
}
void setPwm(float Ua, float Ub, float Uc) {
  // 计算占空比
  // 限制占空比从0到1
  dc_a = _constrain(Ua / voltage_power_supply, 0.0f , 1.0f );
  dc_b = _constrain(Ub / voltage_power_supply, 0.0f , 1.0f );
  dc_c = _constrain(Uc / voltage_power_supply, 0.0f , 1.0f );
  TIM2->CCR1 = (uint16_t)(dc_a*399);
  TIM2->CCR2 = (uint16_t)(dc_b*399);
  TIM2->CCR3 = (uint16_t)(dc_c*399);
}


float _electricalAngle(float shaft_angle,int pole_pairs) 
{
  return (shaft_angle * pole_pairs);
}


float velocityOpenloop(float target_velocity){
     //获取从开启芯片以来的微秒数，它的精度是 4 微秒。 micros() 返回的是一个无符号长整型（unsigned long）的值
  //计算当前每个Loop的运行时间间隔
  float Ts = 0.0005f;
  //由于 micros() 函数返回的时间戳会在大约 70 分钟之后重新开始计数，在由70分钟跳变到0时，TS会出现异常，因此需要进行修正。如果时间间隔小于等于零或大于 0.5 秒，则将其设置为一个较小的默认值，即 1e-3f
//if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
  // 通过乘以时间间隔和目标速度来计算需要转动的机械角度，存储在 shaft_angle 变量中。在此之前，还需要对轴角度进行归一化，以确保其值在 0 到 2π 之间。
  shaft_angle = _normalizeAngle(shaft_angle + target_velocity*Ts);
  //以目标速度为 10 rad/s 为例，如果时间间隔是 1 秒，则在每个循环中需要增加 10 * 1 = 10 弧度的角度变化量，才能使电机转动到目标速度。
  //如果时间间隔是 0.1 秒，那么在每个循环中需要增加的角度变化量就是 10 * 0.1 = 1 弧度，才能实现相同的目标速度。因此，电机轴的转动角度取决于目标速度和时间间隔的乘积。
  // 使用早前设置的voltage_power_supply的1/3作为Uq值，这个值会直接影响输出力矩
  // 最大只能设置为Uq = voltage_power_supply/2，否则ua,ub,uc会超出供电电压限幅
  float Uq = voltage_power_supply/4;
  setPhaseVoltage(Uq,  0, _electricalAngle(shaft_angle, pole_pairs));
  //用于计算下一个时间间隔
  return Uq;
}

//对齐电角度
float FOC_init()
{
  setTorque(3, _3PI_2);
  HAL_Delay(1000);
  float fake_angle = 0;
  zero_electric_angle=_electricalAngle(as5600GetRawAngle(),pole_pairs);
    for (int i = 0; i <= 500; i++) {
    fake_angle= _3PI_2 + _2PI * i / 500.0f;
    setTorque(2,fake_angle);
    HAL_Delay(2);
  }
  AS5600_update();
  float midAngle = AS5600_get_raw();
  for (int i = 500; i >= 0; i--) {
    fake_angle = _3PI_2 + _2PI * i / 500.0f;
    setTorque(2,fake_angle);
    HAL_Delay(2);
  }
  AS5600_update();
  float endAngle = AS5600_get_raw();
  //setPhaseVoltage(0,0,0);
  //HAL_Delay(200);
  if (midAngle == endAngle){
    return -1;
  } else if (midAngle < endAngle){
    PP=-1;
  } else{
    PP=1;
  }
  setTorque(0, _3PI_2);
  return zero_electric_angle;
  //Serial.print("0电角度：");Serial.println(zero_electric_angle);
}

//力矩控制 输入需要控制的力矩以及电角度
void setTorque(float Uq,float angle_el) {
  Uq=_constrain(Uq,-voltage_power_supply/2,voltage_power_supply/2);
  float Ud=0;
  angle_el = _normalizeAngle(angle_el);
  // 帕克逆变换
  Ualpha =  -Uq*sin(angle_el); 
  Ubeta =   Uq*cos(angle_el); 

  // 克拉克逆变换
  Ua = Ualpha + voltage_power_supply/2;
  Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2;
  Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2;
  setPwm(Ua,Ub,Uc);
}



